

#include "assit_func.h"

#include "nh_base_alg.h"


uint32_t findpeaks( const int32_t *x,
                    uint16_t _size,
                    uint16_t *_peak_loc,
                    int32_t *peak_left,
                    int32_t *peak_val_shadow,
                    int32_t *peak_right,
                    uint8_t *res_keep,
                    uint32_t distance)
{
    uint32_t count = 0; // 用于最大值的数量
    // printf("%s,line %d\n",__FILE__,__LINE__);
    count = local_maxima_1d(x,_size,_peak_loc,peak_left,peak_right); /// 返回局部最大值的数量
    
    // printf("local_maxima_1d [ret]->[%d]\n",count);
    // for(uint16_t i=0;i<count+1;i++)
    // {
    //     printf("[%d]->[%d]\n",i,peak_right[i]);
    // }


    for(uint32_t i=0;i<count;i++)
    {
        peak_left[i] = x[ _peak_loc[i] ];
    }
    // printf("%s,line %d\n",__FILE__,__LINE__);
    select_by_peak_distance(_peak_loc,peak_left,peak_val_shadow,peak_right,res_keep,count,distance);
    // printf("%s,line %d\n",__FILE__,__LINE__);

    // printf("select_by_peak_distance [ret]->[%d]\n",count);


    uint32_t self_index=0;
    for(uint32_t i=0;i<count;i++)
    {
        // printf("out res_keep[%d]->[%d] _peak_loc[%d]->[%d]\n",i,res_keep[i],i, _peak_loc[i]);
        if(res_keep[i]==1)
        {   
            peak_right[self_index] = _peak_loc[i];; /// 保存peak
            self_index++;
        }
    }
    // for(uint32_t i=0;i<self_index;i++)
    // {
    //     printf("-->check->> [%d]->[%d]\n",i,peak_right[i]);
    // }
    // printf("-->check->> peak_cnt %d\n",self_index);
    return self_index;
}


uint32_t local_maxima_1d(   const int32_t *x, 
                            uint32_t n, 
                            uint16_t *temp_midpoints, 
                            int32_t *temp_left_edges, 
                            int32_t *temp_right_edges)
{        
    uint32_t count = 0; // 用于记录有效最大值的数量
    uint32_t ahead;
    for (uint32_t i = 1; i < n - 1; ++i) {
        if (x[i - 1] < x[i]) {  /// 前向小于后向
            ahead = i + 1;
            while (ahead < n && x[ahead] == x[i]) /// 平坦则向后延伸
            {
                ++ahead;
            }
            if (x[ahead] < x[i]) {  /// 后向小于前向
                temp_left_edges[count] = i;
                temp_right_edges[count] = ahead - 1;
                temp_midpoints[count] = (temp_left_edges[count] + temp_right_edges[count]) / 2;
                count++;
                i = ahead; // 跳过不可能是最大值的样本
            }
        }
    }
    // 将结果数组的大小设置为实际找到的最大值数量
    temp_midpoints[count]   = 0X7FFF;
    temp_left_edges[count]  = 0X7FFF;
    temp_right_edges[count] = 0X7FFF; // 设置哨兵值以便于后续处理
    return count;
}



/**
 * @brief 
 * 
 * @param _peak_loc index info 
 * @param _peak_val val of index
 * @param _peak_val_shadow  temp for val space
 * @param _peak_val_arg_cache res temo space
 * @param _res_keep res keep list
 * @param _size 
 * @param distance 
 * @return uint32_t keep_size 
 */
uint32_t select_by_peak_distance(uint16_t *_peak_loc ,
                                int32_t *_peak_val,
                                int32_t *_peak_val_shadow,
                                int32_t *_peak_val_arg_cache,
                                uint8_t *_res_keep,
                                uint32_t _size,
                                uint32_t distance)
{
    uint16_t keep_count = 0;
    // printf("select_by_peak_distance  [arg]->[size] = %d\n",_size);
    for(uint32_t i=0;i<_size;i++)
    {
        _res_keep[i]=1;  // clear status
        // printf();
    }
    // printf("%s\tline %d\n",__FILE__,__LINE__);
    argsort(_peak_val,_peak_val_shadow,_size,_peak_val_arg_cache);
    // printf("%s\tline %d\n",__FILE__,__LINE__);
    int32_t j,k;
    // printf("select_by_peak_distance  [arg]->[distance] = %d\n",distance);
    for (int32_t i = _size - 1; i >= 0; --i) 
    {
        j = _peak_val_arg_cache[i];
        if (_res_keep[j]==0) continue; // 如果已经标记为不保留，则跳过
        // 遍历当前峰值之前的峰值，确保它们之间的距离至少为distance_



        // for ( k = j - 1; k >= 0 && ((_peak_loc[j] - _peak_loc[k]) < (int32_t)distance); --k) 
        // {
        //     /// display all var in loop 
        //     _res_keep[k] = 0;
        // }
        // // 遍历当前峰值之后的峰值，确保它们之间的距离至少为distance_
        // for ( k = j + 1; k < _size && ((_peak_loc[k] - _peak_loc[j]) <= (int32_t)distance); ++k) {
        //     _res_keep[k] = 0;
        // }

        k = j-1;
        while ( 0<=k && (_peak_loc[j] - _peak_loc[k]) < (int32_t)distance)
        {
            // printf("k =%d _peak_loc[%d]<%d> - _peak_loc[%d]<%d> = %d\n",k,j,_peak_loc[j],k,_peak_loc[k],_peak_loc[j] - _peak_loc[k]);
            _res_keep[k] = 0;
            k -= 1;
        }
        k = j+1;
        while( k < _size && ((_peak_loc[k] - _peak_loc[j]) < (int32_t)distance))
        {
            _res_keep[k] = 0;
            k += 1;
        }
   
   
    }

    // printf("%s\tline %d\n",__FILE__,__LINE__);
    // 计算保留的峰值数量
    keep_count = 0;
    for (uint32_t i = 0; i < _size; ++i) 
    {
        // printf("res_keep[%d] = %d\n",i,_res_keep[i]);
        if (_res_keep[i]==1) { keep_count++; }
    }
    return keep_count;
}

// 交换两个元素的值
static int32_t temp_swap ;
void swap(int32_t *a, int32_t *b) {
    temp_swap = *a;
    *a = *b;
    *b = temp_swap;
}


uint32_t detrend_move(int32_t *rawlist,uint32_t raw_size,int32_t *detrended_list, int32_t *peak_info, uint32_t peak_size)
{
    uint32_t ret = 0;
    if(peak_size==3)
    {   
        uint32_t index=0;
        int32_t xxint[3];
        int32_t yyint[3];
        int32_t end_base=0;
        
        for(index=0;index<3;index++) //// 加载拟合数据
        {
            xxint[index] = peak_info[index];
            yyint[index] = rawlist[peak_info[index]];
        }
        // yyint[1]  /// 基线基准
        for(index=0;index<raw_size;index++)
        {
            end_base = fitval(xxint,yyint,3,index)-yyint[1];
            detrended_list[index] = rawlist[index]-end_base;
        }
        ret=1;
    }else if(peak_size>3)
    {
//        
//        float xx[4];  // 已知的x值  loc
//        float yy[4];  // 已知的y值  val
//        float divided_diff[4]; // 存储差分的数组
        ret =1;
    }else
    {
        return 0;
    }
		return ret;
}


void detrend_move_test(void)
{
    int32_t sample_val[]={
        82407, 82703, 82986, 83233, 83431, 83579, 83681, 83732,
        83711, 83579, 83302, 82864, 82285, 81621, 80955, 80375,
        79952, 79730, 79721, 79907, 80236, 80632, 81019, 81336,
        81552, 81666, 81701, 81695, 81689, 81715, 81788, 81909,
        82074, 82274, 82497, 82731, 82965, 83190, 83396, 83579,
        83740, 83869, 83928, 83854, 83582, 83081, 82376, 81543,
        80696, 79952, 79406, 79117, 79101, 79328, 79732, 80235,
        80758, 81227, 81582, 81791, 81854, 81805, 81700, 81598,
        81541, 81557, 81662, 81854, 82115, 82418, 82730, 83024,
        83281, 83492, 83655, 83773, 83837, 83814, 83656, 83317,
        82782, 82078, 81274, 80463, 79738, 79173, 78821, 78699,
        78790, 79053, 79432, 79867, 80295, 80660, 80931, 81099,
        81184, 81217, 81239, 81279
    };
    uint32_t _len_sample = sizeof(sample_val)/sizeof(sample_val[0]);
    int32_t result_list[200];
    int32_t velly_info[5] = {18,  52,  87,0};
    uint32_t velly_size = 3;
 /****** 预备的测试数据    *****/

    detrend_move( sample_val,_len_sample,result_list,velly_info,velly_size );
    // printf("detrend_move_test peak_size: %d\n",velly_size);
    for(velly_size=0;velly_size < _len_sample;velly_size++)
    {
       // printf("%d,%d,\n",sample_val[velly_size],result_list[velly_size]);
    }
}




/**

//// 原始数据
/////  长度信息  6    6     4   6    
->[1] red_peak: [ 21]	red_velly:[ 30]	IR_peak:[ 40]	IR_velly:[ 31]
->[2] red_peak: [ 53]	red_velly:[ 61]	IR_peak:[ 81]	IR_velly:[ 61]
->[3] red_peak: [ 81]	red_velly:[ 90]	IR_peak:[112]	IR_velly:[ 91] 
->[4] red_peak: [112]	red_velly:[121]	IR_peak:[143]	IR_velly:[120]
->[5] red_peak: [141]	red_velly:[153]	IR_peak:[  0]	IR_velly:[150]
->[6] red_peak: [169]	red_velly:[179]	IR_peak:[  0]	IR_velly:[179]


range = 5;  //// peak与peak间隔的限制值
// peak_diff > (diff time [31]*0.9) ///无需关系
->[1] red_peak: [  81]	red_velly:[ 90]	    IR_peak:[ 81]	IR_velly:[  91]
->[2] red_peak: [ 112]	red_velly:[ 121]	IR_peak:[ 112]	IR_velly:[ 120]

**/








